package com.dizner.androidcartest;

import android.app.Activity;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;

import java.io.IOException;
import java.util.LinkedHashMap;
import java.util.List;
import java.util.concurrent.TimeUnit;

import android.os.MessageQueue;
import android.os.SystemClock;
import android.text.TextUtils;
import android.util.Log;

import com.google.android.things.contrib.driver.button.Button;
import com.google.android.things.pio.Gpio;
import com.google.android.things.pio.I2cDevice;
import com.google.android.things.pio.PeripheralManager;
import com.google.android.things.pio.Pwm;

import org.greenrobot.eventbus.EventBus;

public class MainActivity extends Activity implements SocketNetworkService.OnMessageListener, SensorEventListener {
    private static final String TAG = MainActivity.class.getSimpleName();


    private LinkedHashMap<Long, String> InstructionMap = new LinkedHashMap<>();
    private RaspberryPI3CarDiver diver1;
    private RaspberryPI3CarDiver diver2;
    private static final int def_miles = 50;//控制消息发送默认间隔时间
    private MotorHAT mMotorHat;
    private PeripheralManager mPm;


    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        SocketNetworkService.getInstance().startServer();
        SocketNetworkService.getInstance().register(this);

        mPm = PeripheralManager.getInstance();

        initHAT();
////
        initEngine();

        initHW();
//        AutoPlayManager.getInstance(mMotorHat).run();

//        initHcsr04();
//        initSound();
    }

    private void initHW() {
        HWSensor.getInstance("BCM14", RaspberryPI3CarDiver.HW_SENSOR_LEFT).register(diver1, diver2).run();
        HWSensor.getInstance("BCM16", RaspberryPI3CarDiver.HW_SENSOR_RIGHT).register(diver1, diver2).run();

    }

    private void initHAT() {

        try {
            mMotorHat = new MotorHAT("I2C1", 0x60);
//            mMotorHat.getSteeringEngine().write(100);
//            SystemClock.sleep(2000);
//            mMotorHat.getSteeringEngine(1).write(90);
//            SystemClock.sleep(2000);
//            mMotorHat.getSteeringEngine(1).write(180);
//            SystemClock.sleep(2000);
//            mMotorHat.getSteeringEngine(1).write(135);
//            SystemClock.sleep(2000);
//            mMotorHat.getSteeringEngine(1).write(0);
//            SystemClock.sleep(2000);
//            mMotorHat.getSteeringEngine(1).write(80);
//            mMotorHat.getMotor(2).setSpeed(100);
//            mMotorHat.getMotor(2).commond(MotorHAT.MOTOR_STATE_CW);
        } catch (IOException e) {
        }
    }

    private void initSound() {
//        PeripheralManager pm = PeripheralManager.getInstance();

    }

    private SensorManager mSensorManager;


    private void initHcsr04() {
//        try {
//            Hcsr04.getInstance("", "BCM7", "BCM8").register().run();
//
//        } catch (IOException e) {
//            e.printStackTrace();
//        }
    }

    private void initEngine() {
        diver1 = RaspberryPI3CarDiver.create(mMotorHat, 1, 2);
        diver2 = RaspberryPI3CarDiver.create(mMotorHat, 3, 4);
        diver1.stop();
        diver2.stop();
    }

    @Override
    protected void onDestroy() {
        super.onDestroy();
    }


    @Override
    public void onMessage(BaseMessage msg) {

        if (msg.getMsgType() == SocketNetworkService.Msg_Type.MSG_CONTROL) {
            Log.i(TAG, "收到未分类控制信息：" + msg.getMsgBody());

            switch (filterDigit(msg.getMsgBody())) {
                case "LEFT":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制信息：" + msg.getMsgBody()).toString());
                    if (AutoPlayManager.getInstance(mMotorHat).isAutoRunning()) {
                        return;
                    }
                    if (diver1 == null || diver2 == null) {
                        return;
                    }
                    diver1.right(def_miles);
                    diver2.right(def_miles);
                    break;
                case "LEFT_END":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制信息：" + msg.getMsgBody()).toString());
                    if (AutoPlayManager.getInstance(mMotorHat).isAutoRunning()) {
                        return;
                    }
                    if (diver1 == null || diver2 == null) {
                        return;
                    }
                    diver1.rightEnd();
                    diver2.rightEnd();
                    break;
                case "RIGHT":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制信息：" + msg.getMsgBody()).toString());
                    if (AutoPlayManager.getInstance(mMotorHat).isAutoRunning()) {
                        return;
                    }
                    if (diver1 == null || diver2 == null) {
                        return;
                    }
                    diver1.left(def_miles);
                    diver2.left(def_miles);
                    break;
                case "RIGHT_END":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制信息：" + msg.getMsgBody()).toString());
                    if (AutoPlayManager.getInstance(mMotorHat).isAutoRunning()) {
                        return;
                    }
                    if (diver1 == null || diver2 == null) {
                        return;
                    }
                    diver1.leftEnd();
                    diver2.leftEnd();
                    break;
                case "FORWARD":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制信息：" + msg.getMsgBody()).toString());
                    if (AutoPlayManager.getInstance(mMotorHat).isAutoRunning()) {
                        return;
                    }
                    if (diver1 == null || diver2 == null) {
                        return;
                    }
                    diver1.forward(def_miles);
                    diver2.forward(def_miles);
                    break;
                case "FORWARD_END":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制信息：" + msg.getMsgBody()).toString());
                    if (AutoPlayManager.getInstance(mMotorHat).isAutoRunning()) {
                        return;
                    }
                    if (diver1 == null || diver2 == null) {
                        return;
                    }
                    diver1.forwardEnd();
                    diver2.forwardEnd();
                    break;
                case "BACK":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制信息：" + msg.getMsgBody()).toString());
                    if (AutoPlayManager.getInstance(mMotorHat).isAutoRunning()) {
                        return;
                    }
                    if (diver1 == null || diver2 == null) {
                        return;
                    }
                    diver1.back(def_miles);
                    diver2.back(def_miles);
                    break;
                case "BACK_END":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制信息：" + msg.getMsgBody()).toString());
                    if (AutoPlayManager.getInstance(mMotorHat).isAutoRunning()) {
                        return;
                    }
                    if (diver1 == null || diver2 == null) {
                        return;
                    }
                    diver1.backEnd();
                    diver2.backEnd();
                    break;
                case "READY":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制信息：" + msg.getMsgBody()).toString());
                    if (AutoPlayManager.getInstance(mMotorHat).isAutoRunning()) {
                        return;
                    }
                    if (diver1 == null || diver2 == null) {
                        return;
                    }
                    diver1.ready();
                    diver2.ready();
                    break;
                case "STOP":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制信息：" + msg.getMsgBody()).toString());
                    if (AutoPlayManager.getInstance(mMotorHat).isAutoRunning()) {
                        return;
                    }
                    if (diver1 == null || diver2 == null) {
                        return;
                    }
                    diver1.stop(def_miles);
                    diver2.stop(def_miles);
                    break;
                case "STOP_END":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制信息：" + msg.getMsgBody()).toString());
                    if (AutoPlayManager.getInstance(mMotorHat).isAutoRunning()) {
                        return;
                    }
                    if (diver1 == null || diver2 == null) {
                        return;
                    }
                    diver1.stopEnd();
                    diver2.stopEnd();
                    break;
                case "LEFT_START":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制信息：" + msg.getMsgBody()).toString());
                    if (AutoPlayManager.getInstance(mMotorHat).isAutoRunning()) {
                        return;
                    }
                    if (diver1 == null || diver2 == null) {
                        return;
                    }
                    diver1.ready();
                    diver2.ready();
                    break;
                case "LEFT_STOP":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制信息：" + msg.getMsgBody()).toString());
                    if (AutoPlayManager.getInstance(mMotorHat).isAutoRunning()) {
                        return;
                    }
                    if (diver1 == null || diver2 == null) {
                        return;
                    }
                    diver1.stop();
                    diver2.stop();
                    break;
                case "RIGHT_START":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制信息：" + msg.getMsgBody()).toString());
                    if (AutoPlayManager.getInstance(mMotorHat).isAutoRunning()) {
                        return;
                    }
//                    diver2.ready();
                    break;
                case "RIGHT_STOP":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制信息：" + msg.getMsgBody()).toString());
                    if (AutoPlayManager.getInstance(mMotorHat).isAutoRunning()) {
                        return;
                    }
//                    diver2.closed();
                    break;
                case "SPEED":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制速度信息：" + filterString(msg.getMsgBody())).toString());
                    if (diver1 == null || diver2 == null) {
                        return;
                    }
                    diver1.changeSpeed(filterString(msg.getMsgBody()));
                    diver2.changeSpeed(filterString(msg.getMsgBody()));
                    break;
                case "SERVO":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制速度信息：" + filterString(msg.getMsgBody())).toString());
                    if (AutoPlayManager.getInstance(mMotorHat).isAutoRunning()) {
                        return;
                    }
                    float v = filterString(msg.getMsgBody());
                    mMotorHat.getSteeringEngine().write(v);
                    break;
                case "AUTO_START":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制速度信息：" + msg.getMsgBody()).toString());
                    AutoPlayManager.getInstance(mMotorHat).run();
                    break;
                case "AUTO_STOP":
//                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制速度信息：" + msg.getMsgBody()).toString());
                    AutoPlayManager.getInstance(mMotorHat).stop();
                    break;
            }
        } else if (msg.getMsgType() == SocketNetworkService.Msg_Type.MSG_SYSTEM) {
            Log.i(TAG, "收到系统信息：" + msg.getMsgBody());
            switch (msg.getMsgBody()) {

                case "HELLO":
                    InstructionMap.put(msg.getMsgId(), msg.getMsgBody());
                    //第一位：左引擎使能，第二\三位：当前速度 ...
//                    SocketNetworkService.getInstance().sendMsg(new ResponseMessage(msg.getMsgId(), "收到控制速度信息：" + filterString(msg.getMsgBody())).toString());
                    break;
            }
        }
        Log.i(TAG, msg.getMsgBody());
    }

    private String filterDigit(String msgBody) {
        if (TextUtils.isEmpty(msgBody)) {
            return "";
        }

        String replace = msgBody.replaceAll("\\d", "");
        Log.i(TAG, "过滤数字：" + replace);
        return replace;
    }

    private float filterString(String msgBody) {
        if (TextUtils.isEmpty(msgBody)) {
            return 0;
        }

        String replace = msgBody.replaceAll("[A-Za-z]", "");
        Log.i(TAG, "过滤字符：" + replace);
        if (!TextUtils.isEmpty(replace)) {
            return Float.parseFloat(replace);
        }

        return 0;
    }

    @Override
    public void onConnect() {

    }

    @Override
    public void onDisconnect() {

    }


    @Override
    public void onSensorChanged(SensorEvent sensorEvent) {
        float distanceInCm = sensorEvent.values[0];
        Log.i(TAG, "distanceInCm = " + sensorEvent.values[0] + "cm");
//        if (distanceInCm < 15) {
//            diver1.setEnableForward(false);
//            diver1.setEnableLeft(false);
//            diver1.setEnableRight(false);
//            diver1.setEnableBack(true);
//        }else {
//            diver1.setEnableForward(true);
//            diver1.setEnableLeft(true);
//            diver1.setEnableRight(true);
//            diver1.setEnableBack(true);
//        }
    }

    @Override
    public void onAccuracyChanged(Sensor sensor, int i) {

    }

    private class Hcsr04SensorCallback extends SensorManager.DynamicSensorCallback {
        @Override
        public void onDynamicSensorConnected(Sensor sensor) {
            Log.i(TAG, sensor.getName() + " has been connected");
            mSensorManager.registerListener(MainActivity.this, sensor, SensorManager.SENSOR_DELAY_NORMAL);
        }

        @Override
        public void onDynamicSensorDisconnected(Sensor sensor) {
            Log.i(TAG, sensor.getName() + " has been disconnected");
            mSensorManager.unregisterListener(MainActivity.this);
        }
    }

}
